/**
 * Arduino两轮小车
 * 
 * 本程序可不受限制的用于学习和教学，商业用途请联系作者。
 * 本程序不对用户因使用程序而造成任何形式的损失负责。
 * 
 * site: http://www.xpstem.com/maker/arduino-vehicle
 * author: billy zhang
 */
 
#include <Arduino.h>
#include "PS2xReceiver.h"

#define Pressures   false
#define Rumble      false

void PS2xReceiver::setup()
{
  delay(1000);  //added delay to give wireless ps2 module some time to startup, before configuring it
   
    //CHANGES for v1.6 HERE!!! **************PAY ATTENTION*************
    
    //setup pins and settings: GamePad(clock, command, attention, data, Pressures?, Rumble?) check for error
    ps2x_error_ = ps2x_.config_gamepad(clkPin_, cmdPin_, csPin_, datPin_, Pressures, Rumble);
    
    if(ps2x_error_ == 0){
      Serial.print("Found Controller, configured successful ");
      Serial.print("pressures = ");
      if (Pressures)
        Serial.println("true ");
      else
        Serial.println("false");
      Serial.print("rumble = ");
      if (Rumble)
        Serial.println("true)");
      else
        Serial.println("false");
        Serial.println("Try out all the buttons, X will vibrate the controller, faster as you press harder;");
        Serial.println("holding L1 or R1 will print out the analog stick values.");
        Serial.println("Note: Go to www.billporter.info for updates and to report bugs.");
    }  
    else if(ps2x_error_ == 1)
      Serial.println("No controller found, check wiring, see readme.txt to enable debug. visit www.billporter.info for troubleshooting tips");
     
    else if(ps2x_error_ == 2)
      Serial.println("Controller found but not accepting commands. see readme.txt to enable debug. Visit www.billporter.info for troubleshooting tips");
  
    else if(ps2x_error_ == 3)
      Serial.println("Controller refusing to enter Pressures mode, may not support it. ");
    
  //  Serial.print(ps2x.Analog(1), HEX);
    
    ps2x_type_ = ps2x_.readType(); 
    switch(ps2x_type_) {
      case 0:
        Serial.print("Unknown Controller type found ");
        break;
      case 1:
        Serial.print("DualShock Controller found ");
        break;
      case 2:
        Serial.print("GuitarHero Controller found ");
        break;
      case 3:
        Serial.print("Wireless Sony DualShock Controller found ");
        break;
     }

}

void PS2xReceiver::receive()
{
  if (ps2x_error_ != 0)
    return;
    
  ps2x_.read_gamepad(false, ps2x_vibrate_); //read controller and set large motor to spin at 'vibrate' speed

  if(ps2x_.Button(PSB_PAD_DOWN)){   //will be TRUE as long as button is pressed
    backwardPressState_=1;
    angleValue_=0;
  } else if(ps2x_.Button(PSB_PAD_UP)) {   
    forwardPressState_=1;
    angleValue_=0;
  } else if(ps2x_.Button(PSB_PAD_RIGHT)){
    rightPressState_ =1;
    
    if (angleValue_ <20)
      angleValue_ += 6;
    if (angleValue_ < 35)
      angleValue_ += 4;
    else
      angleValue_ += 2;
      
    if (angleValue_>45)
      angleValue_=45;
  } else if(ps2x_.Button(PSB_PAD_LEFT)){
    leftPressState_=1;
    
    if (angleValue_>-20)
      angleValue_-=6;
    if (angleValue_>-35)
      angleValue_-=4;
    else
      angleValue_-=2;
      
    if (angleValue_<-45)
      angleValue_=-45;
  }
   
  if (ps2x_.Button(PSB_CIRCLE)) {
     accelPressState_=1;
     if (accelValue_ < 100)
        accelValue_+=1;
  } else {
    if (accelValue_>0) {
      accelValue_-=1;
    }
  }
  if (ps2x_.Button(PSB_SQUARE)) {
     stopPressState_=1;
     accelValue_=0;
     angleValue_=0;
  }   
}

PS2xReceiver::~PS2xReceiver()
{
}
